from dijkstar import fordijkstar
import numpy as np
import math

import scipy.io


# data = scipy.io.loadmat('test.mat')

# car_file = 'G:/活/2019华为挑战/1-map-training-1/car.txt'
# car = []
# with open(car_file,'r') as file_to_read:
#     a = 1
#     b = 1
#     while True:
#         lines = file_to_read.readline()
#         if not lines:
#             break
#         if a==1:
#             a = 0
#             continue
#         lines = lines.strip()
#         lines = lines.strip('()')
#         lines = lines.strip().split(', ')
#         p_temp = []
#         for num in lines:
#             p_temp.append(float(num))
#         car.append(p_temp)
# cross_file = 'G:/活/2019华为挑战/1-map-training-1/cross.txt'
# cross = []
# with open(cross_file, 'r') as file_to_read:
#     a = 1
#     b = 1
#     while True:
#         lines = file_to_read.readline()
#         if not lines:
#             break
#         if a==1:
#             a = 0
#             continue
#         lines = lines.strip()
#         lines = lines.strip('()')
#         lines = lines.strip().split(', ')
#         p_temp = []
#         for num in lines:
#             p_temp.append(float(num))
#         cross.append(p_temp)
# road_file = 'G:/活/2019华为挑战/1-map-training-1/road.txt'
# road = []
# with open(road_file,'r') as file_to_read:
#     a = 1
#     b = 1
#     while True:
#         lines = file_to_read.readline()
#         if not lines:
#             break
#         if a==1:
#             a = 0
#             continue
#         lines = lines.strip()
#         lines = lines.strip('()')
#         lines = lines.strip().split(', ')
#         p_temp = []
#         for num in lines:
#             p_temp.append(float(num))
#         road.append(p_temp)
def Astar_new(Car, Cross, Road, PositionData, car_chromo):
    # Car = data['Car']
    # Cross = data['Cross']
    # Road = data['Road']
    # PositionData = np.zeros((Road.shape[0], max(Road[:,3]) * 2, max(Road[:, 1])), dtype=np.int)
    # car_chromo = [1001,1,1,0,0,0,2,0,0,0,3,4,5,6,0,0,0,0]
    # PositionData = data['PositionData']
    # car_number = 1001
    car_number = car_chromo[0]

    # 找到car_number当前的路口号，和从哪个路口来的
    # max_car_chromo 表示车走到的当前最大步数
    max_car_chromo = max(car_chromo[2:])
    current_road_number = car_chromo[2:].index(max_car_chromo) + 1
    if max(car_chromo[2:]) == 1:
        pre_road_number = current_road_number
    else:
        pre_road_number = car_chromo[2:].index(max(car_chromo[2:]) - 1) + 1

    # 找车辆的终点
    for_find_car = []
    for i in range(len(Car)):
        for_find_car.append(Car[i][0])
    car_index = for_find_car.index(car_number)
    end_road_number = int(Car[car_index][2])

    # vlist_table = fordijkstar(Car,Cross,Road,car_number)

    def real_get_traj(start, index):
        vlist_table = fordijkstar(Car, Cross, Road, car_number, start)
        traj_list = []

        def get_traj(index):  # 参数是顶点在vlist中的索引
            if (index == start):  # 终点
                traj_list.append(index)
                # print(traj_list[::-1])#反转list
                return
            if (vlist_table[int(index)].dist == float('inf')):
                # print('从起点到该顶点根本没有路径')
                return
            traj_list.append(index)
            get_traj(vlist_table[int(index)].prev)

        get_traj(index)
        return vlist_table[int(index)].dist, traj_list

    def find_node_road_number(for_find_road1, node):
        node_road_number_index = []
        a = current_road_number
        road1_index1 = [i for i, x in enumerate(for_find_road1) if x == a]
        # road1_index1 = for_find_road1.index(self.openList[0].closeList)
        if type(road1_index1) == int and node == Road[road1_index1][5]:
            node_road_number_index = road1_index1
        else:
            for temp_i in road1_index1:
                if node == Road[temp_i][5]:
                    node_road_number_index = temp_i
        if node_road_number_index == []:
            b = node
            road1_index2 = [i for i, x in enumerate(for_find_road1) if x == b]
            # road1_index2 = 7
            if type(road1_index2) == int and current_road_number == Road[road1_index2][5]:
                node_road_number_index = road1_index2
            else:
                for temp_i in road1_index2:
                    if current_road_number == Road[temp_i][5]:
                        node_road_number_index = temp_i
        return node_road_number_index

    class AStar:
        """
        AStar算法的Python3.x实现
        """

        class Node:  # 描述AStar算法中的节点数据
            def __init__(self, point, endPoint, g=0):
                self.point = point  # 自己的坐标,当前路口
                self.father = None  # 父节点
                self.g = g  # g值，g值放路况
                # road_next = vlist_table[point].outList
                # self.h,road_next = real_get_traj(point,endPoint)
                vlist_table = fordijkstar(Car, Cross, Road, car_number, point)
                self.openList = vlist_table[point].outList  # 接下来的路口
                self.closeList = pre_road_number  # 走过的路口
                # 删除openList中closeList的值
                # self.closeList.append(minF)
                if pre_road_number in self.openList:
                    self.openList.remove(pre_road_number)
                # self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) * 10  # 计算h值

        def __init__(self, startPoint, endPoint, passTag=0):
            """
            构造AStar算法的启动条件
            :param map2d: Array2D类型的寻路数组
            :param startPoint: Point或二元组类型的寻路起点
            :param endPoint: Point或二元组类型的寻路终点
            :param passTag: int类型的可行走标记（若地图数据!=passTag即为障碍）
            """
            # 开启表
            self.openList = []
            # 关闭表
            self.closeList = []
            # 道路容量
            self.road_capacity = []
            self.startPoint = startPoint
            self.endPoint = endPoint

        def getMinNode(self):
            """
            获得openlist中F值最小的节点
            :return: Node
            """

            node_road_number = []
            for_find_road1 = []
            for i in range(len(Road)):
                for_find_road1.append(Road[i][4])
            a = current_road_number
            # a = self.openList[1:]
            node_all = {}
            for node in self.openList[0].openList:
                # node = []
                # node = self.openList[0].openList[node_index]
                # for node in a:
                # def real_get_traj(vlist_table, start, index):
                # start = currentNode.point
                # index = self.endPoint.x
                # traj_list = []
                # if node == []:
                #     break
                node_road_number_index = find_node_road_number(for_find_road1, node)
                # 找到道路编号node_road_number
                if current_road_number < node:
                    node_PositionData = PositionData[node_road_number_index, 0:Road[node_road_number_index][3], :]
                else:
                    node_PositionData = PositionData[node_road_number_index, Road[node_road_number_index][3]:, :]
                # car_index = for_find_car.index(car_number)
                # 道路容量
                # print(type(node_PositionData))
                # print(np.sum(node_PositionData==0))
                road_capacity = np.sum(node_PositionData == 0) / (
                            Road[node_road_number_index][3] * Road[node_road_number_index][1])

                value_g = abs(Car[car_index][3] - Road[node_road_number_index][2]) + Road[node_road_number_index][
                    3] + math.pow(road_capacity, 1)
                # 字典node_all中外面的关键字是路口号，里面的值是原始G和H
                value_h, road_serial = real_get_traj(node, self.endPoint)
                node_all[node] = [value_g, value_h]
                # len(node_all)

                # real_get_traj(vlist_table, 1, 3)
                # if node.h < currentNode.h:
                #     currentNode = node
            currentNode = self.openList[0].openList[0]
            currentNode_value = node_all[currentNode]
            for min_index in range(len(node_all)):
                temp_node = self.openList[0].openList[min_index]
                temp_node_value = node_all[temp_node]
                if temp_node_value[0] + temp_node_value[1] < currentNode_value[0] + currentNode_value[1]:
                    currentNode = temp_node
                    currentNode_value = temp_node_value
            return currentNode

        def start(self):
            """
            开始寻路
            :return: None或Point列表（路径）
            """
            # # 判断寻路终点是否是障碍
            # if self.map2d[self.endPoint.x][self.endPoint.y] != self.passTag:
            #     return None

            # 1.将起点放入开启列表
            startNode = AStar.Node(self.startPoint, self.endPoint)
            self.openList.append(startNode)
            # 2.主循环逻辑
            while True:
                # 找到F值最小的点
                minF = self.getMinNode()
                # 把这个点加入closeList中，并且在openList中删除它
                return minF

    # 创建AStar对象,并设置起点为当前路口 终点为end_road_number  是路口号
    aStar = AStar(current_road_number, end_road_number)
    # 开始寻路
    nextpath = aStar.start()
    car_chromo[nextpath + 1] = max_car_chromo + 1
    # print(pathList.next_road_number)
    # print(car_chromo)
    return car_chromo
